Gauss-Newton solver using sparse linear solvers through Eigen (no ceres dependency) More...

#include <core/solver/solver_eigen.h>

Inheritance diagram for wolf::SolverEigen:

Classes
struct	SuiteSparseSolvers

Public Types
enum class	LinearSolverType { QR , CHOLESKY }

Public Types inherited from wolf::SolverManager
enum class	CovarianceBlocksToBeComputed : std::size_t { ALL = 0 , ALL_MARGINALS = 1 , ROBOT_LANDMARKS = 2 , GAUSS = 3 , GAUSS_TUCAN = 4 , GAUSS_TUCAN_ONLY_POSITION = 5 }
	Enumeration of covariance blocks to be computed. More...

enum class	ReportVerbosity : std::size_t { QUIET = 0 , BRIEF = 1 , FULL = 2 }
	Enumeration for the verbosity of the solver report. More...

Public Member Functions
	SolverEigen (const ProblemPtr &_wolf_problem, const YAML::Node &_params)

	WOLF_SOLVER_CREATE (SolverEigen)

	~SolverEigen () override

bool	computeCovariancesDerived (const CovarianceBlocksToBeComputed _blocks) override

bool	computeCovariancesDerived (const std::vector< StateBlockPtr > &st_list) override

bool	converged () const override

bool	failed () const override

bool	wasStopped () const override

unsigned int	iterations () const override

double	initialCost () const override

double	finalCost () const override

double	totalTime () const override

int	numStateBlocksEstimatedDerived () const override

int	sizeStateBlocksEstimatedDerived () const override

int	numFactorsDerived () const override

int	sizeFactorsDerived () const override

LinearSolverType	getLinearSolverType () const

std::string	getLinearSolverName () const
	Linear solver and its implementation, e.g. "cholesky (CHOLMOD)" or "qr (Eigen SparseQR)". More...

const Eigen::SparseMatrix< double > &	getJacobian () const
	Stacked jacobian of the last linearization (with loss function weights) More...

std::string	terminationType () const

std::string	briefReport () const
	One line report of the last solve (as ceres::Solver::Summary::BriefReport()) More...

std::string	fullReport () const
	Full report of the last solve (as ceres::Solver::Summary::FullReport()) More...

Public Member Functions inherited from wolf::SolverManager
void	printProfiling (std::ostream &stream=std::cout) const

const ProfilingUnit &	getProfilingTotal () const

const ProfilingUnit &	getProfilingSolve () const

const ProfilingUnit &	getProfilingUpdate () const

const ProfilingUnit &	getProfilingState () const

const ProfilingUnit &	getProfilingCov () const

	SolverManager (const ProblemPtr &_problem)
	Constructor with default params_. More...

	SolverManager (const ProblemPtr &_problem, const YAML::Node &_params)
	Constructor with given params_. More...

virtual	~SolverManager ()

std::string	solve ()
	Solves with the verbosity defined in params_. More...

std::string	solve (const ReportVerbosity report_level)
	Solves with a given verbosity. More...

virtual bool	computeCovariances () final

virtual bool	computeCovariances (const CovarianceBlocksToBeComputed blocks) final

virtual bool	computeCovariances (const std::vector< StateBlockPtr > &st_list) final

virtual void	update ()
	Updates solver's problem according to the wolf_problem. More...

ProblemPtr	getProblem ()

YAML::Node	getParams () const

bool	getComputeCov () const

double	getPeriod () const

double	getCovPeriod () const

ReportVerbosity	getVerbosity () const

virtual bool	isStateBlockRegistered (const StateBlockPtr &state_ptr) const final

virtual bool	isStateBlockFloating (const StateBlockPtr &state_ptr) const final

virtual bool	isStateBlockFixed (const StateBlockPtr &st) const final

virtual bool	isFactorRegistered (const FactorBasePtr &fac_ptr) const final

virtual bool	hasThisLocalParametrization (const StateBlockPtr &st, const LocalParametrizationBasePtr &local_param) const final

virtual bool	hasLocalParametrization (const StateBlockPtr &st) const final

virtual int	numFactors () const final

virtual int	sizeFactors () const final

virtual int	numStateBlocks () const final

virtual int	sizeStateBlocks () const final

virtual int	numStateBlocksEstimated () const final

virtual int	sizeStateBlocksEstimated () const final

virtual int	numFactorsDerived () const =0

virtual int	sizeFactorsDerived () const =0

virtual int	numStateBlocksEstimatedDerived () const =0

virtual int	sizeStateBlocksEstimatedDerived () const =0

virtual bool	check (std::string prefix="") const final

virtual bool	converged () const =0

virtual bool	failed () const =0

virtual bool	wasStopped () const =0

virtual unsigned int	iterations () const =0

virtual double	initialCost () const =0

virtual double	finalCost () const =0

virtual double	totalTime () const =0

virtual void	run () final

virtual void	stop () final

Protected Member Functions
std::string	solveDerived (const ReportVerbosity report_level) override

void	addFactorDerived (const FactorBasePtr &fac_ptr) override

void	removeFactorDerived (const FactorBasePtr &fac_ptr) override

void	addStateBlockDerived (const StateBlockPtr &state_ptr) override

void	removeStateBlockDerived (const StateBlockPtr &state_ptr) override

void	updateStateBlockStatusDerived (const StateBlockPtr &state_ptr) override

void	updateStateBlockLocalParametrizationDerived (const StateBlockPtr &state_ptr) override

bool	isFactorRegisteredDerived (const FactorBasePtr &fac_ptr) const override

bool	isStateBlockRegisteredDerived (const StateBlockPtr &state_ptr) const override

bool	isStateBlockFixedDerived (const StateBlockPtr &st) const override

bool	hasThisLocalParametrizationDerived (const StateBlockPtr &st, const LocalParametrizationBasePtr &local_param) const override

bool	hasLocalParametrizationDerived (const StateBlockPtr &st) const override

void	printProfilingDerived (std::ostream &_stream) const override

bool	checkDerived (std::string prefix="") const override

void	reindex ()
	Recomputes the column of each non-fixed state block and the row of each factor (if structure changed) More...

bool	linearize (bool _apply_loss)

bool	factorize ()
	Factorizes A_ with the selected linear solver. Returns false if it failed or A_ is rank deficient. More...

Eigen::VectorXd	solveIncrement () const
	Solves A_·dx = -b_ using the last factorization. More...

Eigen::MatrixXd	solveNormalEquations (const Eigen::MatrixXd &_E) const
	Solves (AᵀA)·X = E using the last factorization. More...

bool	computeCovarianceBlocks (const std::vector< std::pair< StateBlockPtr, StateBlockPtr > > &_pairs)
	Computes and stores in the problem the covariance blocks of the given pairs of state blocks. More...

void	applyIncrement (const Eigen::VectorXd &_dx)
	Applies an increment (in the tangent space) to the non-fixed state blocks. More...

Protected Member Functions inherited from wolf::SolverManager
virtual Eigen::VectorXd &	getAssociatedMemBlock (const StateBlockPtr &state_ptr)

const double *	getAssociatedMemBlockPtr (const StateBlockPtr &state_ptr) const

double *	getAssociatedMemBlockPtr (const StateBlockPtr &state_ptr)

virtual std::string	solveDerived (const ReportVerbosity report_level)=0

virtual bool	computeCovariancesDerived (const CovarianceBlocksToBeComputed blocks)=0

virtual bool	computeCovariancesDerived (const std::vector< StateBlockPtr > &st_list)=0

virtual void	addFactorDerived (const FactorBasePtr &fac_ptr)=0

virtual void	removeFactorDerived (const FactorBasePtr &fac_ptr)=0

virtual void	addStateBlockDerived (const StateBlockPtr &state_ptr)=0

virtual void	removeStateBlockDerived (const StateBlockPtr &state_ptr)=0

virtual void	updateStateBlockStatusDerived (const StateBlockPtr &state_ptr)=0

virtual void	updateStateBlockLocalParametrizationDerived (const StateBlockPtr &state_ptr)=0

virtual bool	isStateBlockRegisteredDerived (const StateBlockPtr &state_ptr) const =0

virtual bool	isFactorRegisteredDerived (const FactorBasePtr &fac_ptr) const =0

virtual bool	isStateBlockFixedDerived (const StateBlockPtr &st) const =0

virtual bool	hasLocalParametrizationDerived (const StateBlockPtr &st) const =0

virtual bool	hasThisLocalParametrizationDerived (const StateBlockPtr &st, const LocalParametrizationBasePtr &local_param) const =0

virtual void	printProfilingDerived (std::ostream &stream=std::cout) const =0

virtual bool	checkDerived (std::string prefix="") const =0

Protected Attributes
std::map< StateBlockPtr, unsigned int >	sb_2_col_
	first column of each non-fixed state block More...

std::map< FactorBasePtr, unsigned int >	fac_2_row_
	first row of each factor More...

std::map< StateBlockPtr, LocalParametrizationBasePtr >	sb_2_local_param_
	local param of each state block More...

std::set< StateBlockPtr >	fixed_state_blocks_
	registered fixed state blocks More...

bool	structure_dirty_

unsigned int	n_rows_

unsigned int	n_cols_

Eigen::SparseMatrix< double >	A_
	stacked jacobian (weighted by the loss function if applicable) More...

Eigen::VectorXd	b_
	stacked residuals (weighted by the loss function if applicable) More...

double	cost_

LinearSolverType	linear_solver_

bool	analysis_dirty_
	the structure changed: symbolic analysis of the factorization needed More...

Eigen::SparseQR< Eigen::SparseMatrix< double >, Eigen::NaturalOrdering< int > >	qr_
	columns ordered in reindex() More...

Eigen::SimplicialLDLT< Eigen::SparseMatrix< double > >	ldlt_

std::unique_ptr< SuiteSparseSolvers >	suitesparse_
	SPQR and CHOLMOD (defined only if SuiteSparse was found) More...

Eigen::SparseMatrix< double >	R_

Eigen::SparseMatrix< double >	Rt_
	(sorted) R factor of the QR and its transpose, used for covariances More...

Eigen::PermutationMatrix< Eigen::Dynamic, Eigen::Dynamic, int >	R_perm_
	column permutation of the QR More...

unsigned int	max_num_iterations_

double	function_tolerance_

double	parameter_tolerance_

double	gradient_tolerance_

bool	interrupt_on_problem_change_
	interrupt the solver if the problem changed (as SolverCeres) More...

unsigned int	min_num_iterations_
	iterations during which the solver cannot be interrupted More...

unsigned int	iterations_

double	initial_cost_

double	final_cost_

double	total_time_

bool	converged_

bool	failed_

bool	stopped_

std::string	termination_message_

unsigned int	successful_steps_

unsigned int	unsuccessful_steps_
	accepted and rejected Gauss-Newton steps More...

unsigned int	line_search_steps_
	step halvings of the backtracking line search More...

unsigned int	n_linearizations_

unsigned int	n_linear_solves_

double	preprocessor_time_

double	minimizer_time_

double	postprocessor_time_

double	linearization_time_

double	linear_solver_time_

unsigned int	n_iter_acc_

unsigned int	n_iter_max_

unsigned int	n_convergence_

unsigned int	n_interrupted_

unsigned int	n_failed_

unsigned int	n_no_convergence_

Protected Attributes inherited from wolf::SolverManager
ProblemPtr	wolf_problem_

ProfilingUnit	profiling_total_

ProfilingUnit	profiling_solve_

ProfilingUnit	profiling_update_

ProfilingUnit	profiling_state_

ProfilingUnit	profiling_cov_

YAML::Node	params_

std::map< StateBlockPtr, Eigen::VectorXd >	state_blocks_

std::map< StateBlockPtr, FactorBasePtrList >	state_blocks_2_factors_

std::set< FactorBasePtr >	factors_

std::set< StateBlockPtr >	floating_state_blocks_

double	period_

ReportVerbosity	verbose_

bool	compute_cov_

CovarianceBlocksToBeComputed	cov_enum_

double	cov_period_

std::chrono::high_resolution_clock::time_point	last_cov_time_

bool	stopping_

Additional Inherited Members
Static Public Member Functions inherited from wolf::SolverManager
static SolverManagerPtr	autoSetup (const ProblemPtr &_problem, const std::string &_input_yaml_file, std::vector< std::string > _primary_schema_folders, bool _skip_yaml_validation=false)

static SolverManagerPtr	autoSetup (const ProblemPtr &_problem, YAML::Node _param_node, std::vector< std::string > _primary_schema_folders, bool _skip_yaml_validation=false)

Detailed Description

Gauss-Newton solver using sparse linear solvers through Eigen (no ceres dependency)

At each iteration, all factors are linearized at the current estimate building the stacked (sparse) Jacobian A and the residual vector b. Each factor fills a block row and each non-fixed state block a block column (of the size of its tangent space). The columns are ordered with a fill-reducing ordering (COLAMD on the block structure) and the rows are sorted by their leftmost column. Factors whose state blocks are all fixed only contribute to the cost. The increment is computed solving A·dx = -b:

'cholesky': Cholesky factorization of the normal equations AᵀA·dx = -Aᵀb (default, fastest).
'qr': QR factorization of A (numerically more robust: cond(A) instead of cond(A)²). If SuiteSparse was found at build time, CHOLMOD (supernodal Cholesky) and SPQR (multifrontal QR) are used. Otherwise, Eigen's built-in Eigen::SimplicialLDLT and Eigen::SparseQR (much slower for large problems). If the cost increases, the step is halved until it decreases (backtracking line search, up to 10 times). As SolverCeres, with 'interrupt_on_problem_change' the solver stops (after 'min_num_iterations') as soon as the problem changes, keeping the estimate if the cost decreased (otherwise, the estimate is not updated). The problem has to be fully constrained (e.g. first frame fixed or with a prior), otherwise A is rank deficient and the solver fails.

Factors with 'apply_loss_function' use a Cauchy loss with scale 0.5 (as SolverCeres), applied by reweighting (iteratively reweighted least squares). Covariances are computed without loss function (as SolverCeres) and stored in the tangent space of the state blocks.

Member Enumeration Documentation

◆ LinearSolverType

enum class wolf::SolverEigen::LinearSolverType

strong

Enumerator
QR
CHOLESKY

Constructor & Destructor Documentation

◆ SolverEigen()

wolf::SolverEigen::SolverEigen	(	const ProblemPtr &	_wolf_problem,
		const YAML::Node &	_params
	)

◆ ~SolverEigen()

wolf::SolverEigen::~SolverEigen ( )

override

Member Function Documentation

◆ addFactorDerived()

void wolf::SolverEigen::addFactorDerived ( const FactorBasePtr & fac_ptr )

overrideprotectedvirtual

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Enumeration Documentation

◆ LinearSolverType

Constructor & Destructor Documentation

◆ SolverEigen()

◆ ~SolverEigen()

Member Function Documentation

◆ addFactorDerived()

◆ addStateBlockDerived()

◆ applyIncrement()

◆ briefReport()

◆ checkDerived()

◆ computeCovarianceBlocks()

◆ computeCovariancesDerived() [1/2]

◆ computeCovariancesDerived() [2/2]

◆ converged()

◆ factorize()

◆ failed()

◆ finalCost()

◆ fullReport()

◆ getJacobian()

◆ getLinearSolverName()

◆ getLinearSolverType()

◆ hasLocalParametrizationDerived()

◆ hasThisLocalParametrizationDerived()

◆ initialCost()

◆ isFactorRegisteredDerived()

◆ isStateBlockFixedDerived()

◆ isStateBlockRegisteredDerived()

◆ iterations()

◆ linearize()

◆ numFactorsDerived()

◆ numStateBlocksEstimatedDerived()

◆ printProfilingDerived()

◆ reindex()

◆ removeFactorDerived()

◆ removeStateBlockDerived()

◆ sizeFactorsDerived()

◆ sizeStateBlocksEstimatedDerived()

◆ solveDerived()

◆ solveIncrement()

◆ solveNormalEquations()

◆ terminationType()

◆ totalTime()

◆ updateStateBlockLocalParametrizationDerived()

◆ updateStateBlockStatusDerived()

◆ wasStopped()

◆ WOLF_SOLVER_CREATE()

Member Data Documentation

◆ A_

◆ analysis_dirty_

◆ b_

◆ converged_

◆ cost_

◆ fac_2_row_

◆ failed_

◆ final_cost_

◆ fixed_state_blocks_

◆ function_tolerance_

◆ gradient_tolerance_

◆ initial_cost_

◆ interrupt_on_problem_change_

◆ iterations_

◆ ldlt_

◆ line_search_steps_

◆ linear_solver_

◆ linear_solver_time_

◆ linearization_time_

◆ max_num_iterations_

◆ min_num_iterations_

◆ minimizer_time_

◆ n_cols_

◆ n_convergence_

◆ n_failed_